import cv2
import numpy as np
import glob
 
# 棋盘格尺寸（内角点的行数和列数）
pattern_size = (9, 6)
# 棋盘格每个方块的实际尺寸（mm）
square_size = 28.2
 
# 准备对象点，如 (0,0,0), (1,0,0), (2,0,0) ....,(9,6,0)
objp = np.zeros((pattern_size[0] * pattern_size[1], 3), np.float32)
objp[:, :2] = np.mgrid[0:pattern_size[0], 0:pattern_size[1]].T.reshape(-1, 2) * square_size
 
# 存储对象点和图像点的数组
objpoints = []  # 3D点在真实世界中的坐标
imgpoints = []  # 2D点在图像平面中的坐标
 
# 读取所有图像
images = glob.glob('./zhang/*.jpg')
print(f"找到 {len(images)} 张图像")  # 打印找到的图像数量
 
for fname in images:
    img = cv2.imread(fname)
    if img is None:
        print(f"无法读取图像: {fname}")
        continue
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
 
    # 查找棋盘格角点
    ret, corners = cv2.findChessboardCorners(gray, pattern_size, None)
 
    # 如果找到，添加对象点和图像点
    if ret:
        objpoints.append(objp)
 
        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
        corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
        imgpoints.append(corners2)
 
        # 绘制并显示角点
        cv2.drawChessboardCorners(img, pattern_size, corners2, ret)
        cv2.imshow('img', img)
        cv2.waitKey(500)
 
cv2.destroyAllWindows()
 
print(f"找到 {len(objpoints)} 个有效的棋盘格角点")  # 打印找到的有效棋盘格角点数量
 
# 相机标定
if len(objpoints) > 0:
    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
 
    if ret:
        print("相机内参矩阵 (camera_matrix):")
        print(mtx)
        print("相机畸变系数 (dist_coeffs):")
        print(dist)
    else:
        print("标定失败，请检查图像或棋盘格尺寸。")
else:
    print("没有找到有效的棋盘格角点，请检查图像或棋盘格尺寸。")